#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "unitree_api/msg/request.hpp"
#include "common/ros2_sport_client.h"
#include <optional>
#include <string>
#include <unordered_map>

bool FootAdjust = false;
bool FootFlag = false;
float RaisingHeight = 0.03;
int offline_count = 0;

// 步态模式映射
std::unordered_map<std::string, int> gait_map = {
    {"standard", 1},
    {"highspeed", 2},
    {"terrain", 3}};

class VelocitySubscriber : public rclcpp::Node
{
public:
    VelocitySubscriber()
        : Node("velocity_subscriber_multimode")
    {
        // 声明并获取步态模式参数
        this->declare_parameter<std::string>("mode", "standard");
        std::string mode;
        this->get_parameter("mode", mode);
        if (gait_map.find(mode) != gait_map.end())
        {
            gait_id_ = gait_map[mode];
        }
        else
        {
            RCLCPP_WARN(this->get_logger(), "Unknown mode '%s', fallback to 'standard'", mode.c_str());
            gait_id_ = gait_map["standard"];
        }

        // 声明并获取速度缩放系数参数
        this->declare_parameter<double>("linear_x_scale", 2.0);
        this->declare_parameter<double>("linear_y_scale", 1.0);
        this->declare_parameter<double>("angular_z_scale", 1.0);

        this->get_parameter("linear_x_scale", linear_x_scale_);
        this->get_parameter("linear_y_scale", linear_y_scale_);
        this->get_parameter("angular_z_scale", angular_z_scale_);

        RCLCPP_INFO(this->get_logger(), "Speed scaling factors - X: %.2f, Y: %.2f, Z: %.2f",
                    linear_x_scale_, linear_y_scale_, angular_z_scale_);

        publisher_ = this->create_publisher<unitree_api::msg::Request>("/api/sport/request", 10);
        subscription_ = this->create_subscription<geometry_msgs::msg::Twist>(
            "/cmd_vel", 10,
            [this](geometry_msgs::msg::Twist::UniquePtr msg)
            {
                last_msg_ = *msg;
                offline_count = 0; // 重置离线计数器
            });
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(100),
            std::bind(&VelocitySubscriber::publish_request, this));

        // 启动时切换步态
        unitree_api::msg::Request req;
        SportClient sport_req;
        sport_req.SwitchGait(req, gait_id_);
        publisher_->publish(req);
    }

private:
    void publish_request()
    {
        SportClient sport_req;
        unitree_api::msg::Request req;

        // 检查离线状态
        if (offline_count >= 5)
        {
            RCLCPP_WARN(this->get_logger(), "Command source offline! Sending zero velocity command.");
            sport_req.Move(req, 0.0, 0.0, 0.0);
            publisher_->publish(req);
            return;
        }

        if (!last_msg_)
            return;

        // 应用速度缩放系数
        double scaled_linear_x = last_msg_->linear.x * linear_x_scale_;
        double scaled_linear_y = last_msg_->linear.y * linear_y_scale_;
        double scaled_angular_z = last_msg_->angular.z * angular_z_scale_;

        sport_req.Move(req, scaled_linear_x, scaled_linear_y, scaled_angular_z);
        publisher_->publish(req);

        offline_count++; // 增加离线计数器
    }

    rclcpp::Publisher<unitree_api::msg::Request>::SharedPtr publisher_;
    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr subscription_;
    rclcpp::TimerBase::SharedPtr timer_;
    std::optional<geometry_msgs::msg::Twist> last_msg_;
    int gait_id_;

    // 速度缩放系数
    double linear_x_scale_;
    double linear_y_scale_;
    double angular_z_scale_;
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<VelocitySubscriber>());
    rclcpp::shutdown();
    return 0;
}